
/**
  ******************************************************************************
  * Copyright 2021 The Microbee Authors. All Rights Reserved.
  * 
  * Licensed under the Apache License, Version 2.0 (the "License");
  * you may not use this file except in compliance with the License.
  * You may obtain a copy of the License at
  * 
  * http://www.apache.org/licenses/LICENSE-2.0
  * 
  * Unless required by applicable law or agreed to in writing, software
  * distributed under the License is distributed on an "AS IS" BASIS,
  * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
  * See the License for the specific language governing permissions and
  * limitations under the License.
  * 
  * @file       sensor_gps_backend.h
  * @author     baiyang
  * @date       2022-2-13
  ******************************************************************************
  */

#pragma once

#ifdef __cplusplus
extern "C"{
#endif

/*----------------------------------include-----------------------------------*/
#include <stdint.h>
#include <stdbool.h>

#include <rtdebug.h>

#include <rtc/jitter_correction.h>
#include <serial_manager/gp_serial.h>
#include <common/location/location.h>
#include <common/gp_math/gp_mathlib.h>
#include <gcs_mavlink/gcs_mavlink.h>
/*-----------------------------------macro------------------------------------*/

/*----------------------------------typedef-----------------------------------*/
// GPS driver types
enum GPS_Type {
    GPS_TYPE_NONE  = 0,
    GPS_TYPE_AUTO  = 1,
    GPS_TYPE_UBLOX = 2,
    // GPS_TYPE_MTK   = 3,  // driver removed
    // GPS_TYPE_MTK19 = 4,  // driver removed
    GPS_TYPE_NMEA  = 5,
    GPS_TYPE_SIRF  = 6,
    GPS_TYPE_HIL   = 7,
    GPS_TYPE_SBP   = 8,
    GPS_TYPE_UAVCAN = 9,
    GPS_TYPE_SBF   = 10,
    GPS_TYPE_GSOF  = 11,
    GPS_TYPE_ERB = 13,
    GPS_TYPE_MAV = 14,
    GPS_TYPE_NOVA = 15,
    GPS_TYPE_HEMI = 16, // hemisphere NMEA
    GPS_TYPE_UBLOX_RTK_BASE = 17,
    GPS_TYPE_UBLOX_RTK_ROVER = 18,
    GPS_TYPE_MSP = 19,
    GPS_TYPE_ALLYSTAR = 20, // AllyStar NMEA
    GPS_TYPE_EXTERNAL_AHRS = 21,
    GPS_TYPE_UAVCAN_RTK_BASE = 22,
    GPS_TYPE_UAVCAN_RTK_ROVER = 23,
};

/// GPS status codes
enum GPS_Status {
    NO_GPS = GPS_FIX_TYPE_NO_GPS,                     ///< No GPS connected/detected
    NO_FIX = GPS_FIX_TYPE_NO_FIX,                     ///< Receiving valid GPS messages but no lock
    GPS_OK_FIX_2D = GPS_FIX_TYPE_2D_FIX,              ///< Receiving valid messages and 2D lock
    GPS_OK_FIX_3D = GPS_FIX_TYPE_3D_FIX,              ///< Receiving valid messages and 3D lock
    GPS_OK_FIX_3D_DGPS = GPS_FIX_TYPE_DGPS,           ///< Receiving valid messages and 3D lock with differential improvements
    GPS_OK_FIX_3D_RTK_FLOAT = GPS_FIX_TYPE_RTK_FLOAT, ///< Receiving valid messages and 3D RTK Float
    GPS_OK_FIX_3D_RTK_FIXED = GPS_FIX_TYPE_RTK_FIXED, ///< Receiving valid messages and 3D RTK Fixed
};

// role for auto-config
enum GPS_Role {
    GPS_ROLE_NORMAL,
    GPS_ROLE_MB_BASE,
    GPS_ROLE_MB_ROVER,
};

/*
  The GPS_State structure is filled in by the backend driver as it
  parses each message from the GPS.
 */
struct GPS_State {
    uint8_t instance; // the instance number of this GPS

    // all the following fields must all be filled by the backend driver
    enum GPS_Status status;                  ///< driver fix status
    uint32_t time_week_ms;              ///< GPS time (milliseconds from start of GPS week)
    uint16_t time_week;                 ///< GPS week number
    Location location;                  ///< last fix location
    float ground_speed;                 ///< ground speed in m/sec
    float ground_course;                ///< ground course in degrees
    float gps_yaw;                      ///< GPS derived yaw information, if available (degrees)
    uint32_t gps_yaw_time_ms;           ///< timestamp of last GPS yaw reading
    bool  gps_yaw_configured;           ///< GPS is configured to provide yaw
    uint16_t hdop;                      ///< horizontal dilution of precision in cm
    uint16_t vdop;                      ///< vertical dilution of precision in cm
    uint8_t num_sats;                   ///< Number of visible satellites
    Vector3f_t velocity;                  ///< 3D velocity in m/s, in NED format
    float speed_accuracy;               ///< 3D velocity RMS accuracy estimate in m/s
    float horizontal_accuracy;          ///< horizontal RMS accuracy estimate in m
    float vertical_accuracy;            ///< vertical RMS accuracy estimate in m
    float gps_yaw_accuracy;           ///< heading accuracy of the GPS in degrees
    bool have_vertical_velocity;      ///< does GPS give vertical velocity? Set to true only once available.
    bool have_speed_accuracy;         ///< does GPS give speed accuracy? Set to true only once available.
    bool have_horizontal_accuracy;    ///< does GPS give horizontal position accuracy? Set to true only once available.
    bool have_vertical_accuracy;      ///< does GPS give vertical position accuracy? Set to true only once available.
    bool have_gps_yaw;                ///< does GPS give yaw? Set to true only once available.
    bool have_gps_yaw_accuracy;       ///< does the GPS give a heading accuracy estimate? Set to true only once available
    uint32_t last_gps_time_ms;          ///< the system time we got the last GPS timestamp, milliseconds
    uint32_t uart_timestamp_ms;         ///< optional timestamp from set_uart_timestamp()
    uint32_t lagged_sample_count;       ///< number of samples with 50ms more lag than expected

    // all the following fields must only all be filled by RTK capable backend drivers
    uint32_t rtk_time_week_ms;         ///< GPS Time of Week of last baseline in milliseconds
    uint16_t rtk_week_number;          ///< GPS Week Number of last baseline
    uint32_t rtk_age_ms;               ///< GPS age of last baseline correction in milliseconds  (0 when no corrections, 0xFFFFFFFF indicates overflow)
    uint8_t  rtk_num_sats;             ///< Current number of satellites used for RTK calculation
    uint8_t  rtk_baseline_coords_type; ///< Coordinate system of baseline. 0 == ECEF, 1 == NED
    int32_t  rtk_baseline_x_mm;        ///< Current baseline in ECEF x or NED north component in mm
    int32_t  rtk_baseline_y_mm;        ///< Current baseline in ECEF y or NED east component in mm
    int32_t  rtk_baseline_z_mm;        ///< Current baseline in ECEF z or NED down component in mm
    uint32_t rtk_accuracy;             ///< Current estimate of 3D baseline accuracy (receiver dependent, typical 0 to 9999)
    int32_t  rtk_iar_num_hypotheses;   ///< Current number of integer ambiguity hypotheses
    
    // UBX Relative Position and Heading message information
    float relPosHeading;               ///< Reported Heading in degrees
    float relPosLength;                ///< Reported Position horizontal distance in meters
    float relPosD;                     ///< Reported Vertical distance in meters
    float accHeading;                  ///< Reported Heading Accuracy in degrees
    uint32_t relposheading_ts;        ///< True if new data has been received since last time it was false
};

// GPS navigation engine settings. Not all GPS receivers support
// this
enum GPS_Engine_Setting {
    GPS_ENGINE_NONE        = -1,
    GPS_ENGINE_PORTABLE    = 0,
    GPS_ENGINE_STATIONARY  = 2,
    GPS_ENGINE_PEDESTRIAN  = 3,
    GPS_ENGINE_AUTOMOTIVE  = 4,
    GPS_ENGINE_SEA         = 5,
    GPS_ENGINE_AIRBORNE_1G = 6,
    GPS_ENGINE_AIRBORNE_2G = 7,
    GPS_ENGINE_AIRBORNE_4G = 8
};

enum GpsDriverOptions {
    UBX_MBUseUart2    = (1U << 0U),
    SBF_UseBaseForYaw = (1U << 1U),
    UBX_Use115200     = (1U << 2U),
    UAVCAN_MBUseDedicatedBus  = (1 << 3U),
};

// Auto configure types
enum GPS_AUTO_CONFIG {
    GPS_AUTO_CONFIG_DISABLE = 0,
    GPS_AUTO_CONFIG_ENABLE_SERIAL_ONLY  = 1,
    GPS_AUTO_CONFIG_ENABLE_ALL = 2,
};

enum GPSAutoSwitch {
    GPS_NONE        = 0,
    GPS_USE_BEST    = 1,
    GPS_BLEND       = 2,
    //GPS_USE_SECOND  = 3, deprecated for new primary param
    GPS_USE_PRIMARY_IF_3D_FIX = 4,
};

typedef struct sensor_gps_backend* sensor_gps_backend_t;

/** @ 
  * @brief  
  */
struct sensor_gps_backend {
    struct sensor_gps_backend_ops *ops;

    rt_device_t port;                     ///< UART we are attached to
    struct GPS_State *state;              ///< public state for this instance

    // itow from previous message
    uint32_t _last_itow;
    uint64_t _pseudo_itow;
    uint32_t _last_ms;
    uint32_t _rate_ms;
    uint32_t _last_rate_ms;
    uint16_t _rate_counter;

    JitterCorrection jitter_correction;
};

/** @ 
  * @brief  
  */
struct sensor_gps_backend_ops {
    // we declare a virtual destructor so that GPS drivers can
    // override with a custom destructor if need be.
    void (*gps_backend_destructor)(sensor_gps_backend_t gps_backend);

    // The read() method is the only one needed in each driver. It
    // should return true when the backend has successfully received a
    // valid packet from the GPS.
    bool (*read)(sensor_gps_backend_t gps_backend);

    // Highest status supported by this GPS. 
    // Allows external system to identify type of receiver connected.
    enum GPS_Status (*highest_supported_status)(sensor_gps_backend_t gps_backend);

    bool (*is_configured)(sensor_gps_backend_t gps_backend);

    void (*inject_data)(sensor_gps_backend_t gps_backend, const uint8_t *data, uint16_t len);

    //MAVLink methods
    bool (*supports_mavlink_gps_rtk_message)(sensor_gps_backend_t gps_backend);
    void (*send_mavlink_gps_rtk)(sensor_gps_backend_t gps_backend, mavlink_channel_t chan);

    void (*broadcast_configuration_failure_reason)(sensor_gps_backend_t gps_backend);

    void (*handle_msg)(sensor_gps_backend_t gps_backend, const mavlink_message_t *msg);

    // driver specific lag, returns true if the driver is confident in the provided lag
    bool (*get_lag)(sensor_gps_backend_t gps_backend, float *lag);

    // driver specific health, returns true if the driver is healthy
    bool (*is_healthy)(sensor_gps_backend_t gps_backend);

    // returns true if the GPS is doing any logging it is expected to
    bool (*logging_healthy)(sensor_gps_backend_t gps_backend);

    const char *(*name)();

    void (*Write_AP_Logger_Log_Startup_messages)(sensor_gps_backend_t gps_backend);

    bool (*prepare_for_arming)(sensor_gps_backend_t gps_backend);

    // optional support for retrieving RTCMv3 data from a moving baseline base
    bool (*get_RTCMV3)(sensor_gps_backend_t gps_backend, const uint8_t *bytes, uint16_t *len);
    void (*clear_RTCMV3)(sensor_gps_backend_t gps_backend);

    bool (*get_error_codes)(sensor_gps_backend_t gps_backend, uint32_t *error_codes);
};

/*----------------------------------variable----------------------------------*/

/*-------------------------------------os-------------------------------------*/

/*----------------------------------function----------------------------------*/
void sensor_gps_backend_ctor(sensor_gps_backend_t gps_backend, struct sensor_gps_backend_ops* ops, struct GPS_State *_state, rt_device_t _port);

// common utility functions
int32_t sensor_gps_backend_swap_int32(int32_t v);
int16_t sensor_gps_backend_swap_int16(int16_t v);

/*
  fill in 3D velocity from 2D components
 */
void sensor_gps_backend_fill_3d_velocity(sensor_gps_backend_t gps_backend);

/*
   fill in time_week_ms and time_week from BCD date and time components
   assumes MTK19 millisecond form of bcd_time
*/
void sensor_gps_backend_make_gps_time(sensor_gps_backend_t gps_backend, uint32_t bcd_date, uint32_t bcd_milliseconds);

/*
  set a timestamp based on arrival time on uart at current byte,
  assuming the message started nbytes ago
 */
void sensor_gps_backend_set_uart_timestamp(sensor_gps_backend_t gps_backend, uint16_t nbytes);

void sensor_gps_backend_check_new_itow(sensor_gps_backend_t gps_backend, uint32_t itow, uint32_t msg_length);

/*
  access to driver option bits
 */
enum GpsDriverOptions sensor_gps_backend_driver_options(void);

// get GPS type, for subtype config
enum GPS_Type sensor_gps_backend_get_type(sensor_gps_backend_t gps_backend);

void sensor_gps_backend_broadcast_gps_type(sensor_gps_backend_t gps_backend);


/// 
static inline void sensor_gps_backend_destructor(sensor_gps_backend_t gps_backend)
{
    if (gps_backend != RT_NULL && gps_backend->ops->gps_backend_destructor != RT_NULL) {
        gps_backend->ops->gps_backend_destructor(gps_backend);
    }
}

static inline bool sensor_gps_backend_read(sensor_gps_backend_t gps_backend)
{
    RT_ASSERT(gps_backend != RT_NULL);
    RT_ASSERT(gps_backend->ops->read != RT_NULL);

    return gps_backend->ops->read(gps_backend);
}

static inline enum GPS_Status sensor_gps_backend_highest_supported_status(sensor_gps_backend_t gps_backend)
{
    if (gps_backend != RT_NULL && gps_backend->ops->highest_supported_status != RT_NULL) {
        return gps_backend->ops->highest_supported_status(gps_backend);
    }

    return GPS_OK_FIX_3D;
}

static inline bool sensor_gps_backend_is_configured(sensor_gps_backend_t gps_backend)
{
    if (gps_backend != RT_NULL && gps_backend->ops->is_configured != RT_NULL) {
        return gps_backend->ops->is_configured(gps_backend);
    }

    return true;
}

static inline void sensor_gps_backend_inject_data(sensor_gps_backend_t gps_backend, const uint8_t *data, uint16_t len)
{
    if (gps_backend != RT_NULL && gps_backend->ops->inject_data != RT_NULL) {
        gps_backend->ops->inject_data(gps_backend, data, len);
    }
}

static inline bool sensor_gps_backend_supports_mavlink_gps_rtk_message(sensor_gps_backend_t gps_backend)
{
    if (gps_backend != RT_NULL && gps_backend->ops->supports_mavlink_gps_rtk_message != RT_NULL) {
        return gps_backend->ops->supports_mavlink_gps_rtk_message(gps_backend);
    }

    return false;
}

static inline void sensor_gps_backend_send_mavlink_gps_rtk(sensor_gps_backend_t gps_backend, mavlink_channel_t chan)
{
    if (gps_backend != RT_NULL && gps_backend->ops->send_mavlink_gps_rtk != RT_NULL) {
        gps_backend->ops->send_mavlink_gps_rtk(gps_backend, chan);
    }
}

static inline void sensor_gps_backend_broadcast_configuration_failure_reason(sensor_gps_backend_t gps_backend)
{
    if (gps_backend != RT_NULL && gps_backend->ops->broadcast_configuration_failure_reason != RT_NULL) {
        gps_backend->ops->broadcast_configuration_failure_reason(gps_backend);
    }
}

static inline void sensor_gps_backend_handle_msg(sensor_gps_backend_t gps_backend, const mavlink_message_t *msg)
{
    if (gps_backend != RT_NULL && gps_backend->ops->handle_msg != RT_NULL) {
        gps_backend->ops->handle_msg(gps_backend, msg);
    }
}

static inline bool sensor_gps_backend_get_lag(sensor_gps_backend_t gps_backend, float *lag)
{
    if (gps_backend != RT_NULL && gps_backend->ops->get_lag != RT_NULL) {
        return gps_backend->ops->get_lag(gps_backend, lag);
    }

    *lag = 0.2f; 
    return true;
}

static inline bool sensor_gps_backend_is_healthy(sensor_gps_backend_t gps_backend)
{
    if (gps_backend != RT_NULL && gps_backend->ops->is_healthy != RT_NULL) {
        return gps_backend->ops->is_healthy(gps_backend);
    }

    return true;
}

static inline bool sensor_gps_backend_logging_healthy(sensor_gps_backend_t gps_backend)
{
    if (gps_backend != RT_NULL && gps_backend->ops->logging_healthy != RT_NULL) {
        return gps_backend->ops->logging_healthy(gps_backend);
    }

    return true;
}

static inline const char *sensor_gps_backend_name(sensor_gps_backend_t gps_backend)
{
    RT_ASSERT(gps_backend != RT_NULL);
    RT_ASSERT(gps_backend->ops->name != RT_NULL);

    return gps_backend->ops->name();
}

static inline void sensor_gps_backend_Write_AP_Logger_Log_Startup_messages(sensor_gps_backend_t gps_backend)
{
    if (gps_backend != RT_NULL && gps_backend->ops->Write_AP_Logger_Log_Startup_messages != RT_NULL) {
        gps_backend->ops->Write_AP_Logger_Log_Startup_messages(gps_backend);
    }
}

static inline bool sensor_gps_backend_prepare_for_arming(sensor_gps_backend_t gps_backend)
{
    if (gps_backend != RT_NULL && gps_backend->ops->prepare_for_arming != RT_NULL) {
        return gps_backend->ops->prepare_for_arming(gps_backend);
    }

    return true;
}

static inline bool sensor_gps_backend_get_RTCMV3(sensor_gps_backend_t gps_backend, const uint8_t *bytes, uint16_t *len)
{
    if (gps_backend != RT_NULL && gps_backend->ops->get_RTCMV3 != RT_NULL) {
        return gps_backend->ops->get_RTCMV3(gps_backend, bytes, len);
    }

    return false;
}

static inline void sensor_gps_backend_clear_RTCMV3(sensor_gps_backend_t gps_backend)
{
    if (gps_backend != RT_NULL && gps_backend->ops->clear_RTCMV3 != RT_NULL) {
        gps_backend->ops->clear_RTCMV3(gps_backend);
    }
}

static inline bool sensor_gps_backend_get_error_codes(sensor_gps_backend_t gps_backend, uint32_t *error_codes)
{
    if (gps_backend != RT_NULL && gps_backend->ops->get_error_codes != RT_NULL) {
        return gps_backend->ops->get_error_codes(gps_backend, error_codes);
    }

    return false;
}

/*------------------------------------test------------------------------------*/

#ifdef __cplusplus
}
#endif



